/*
Arm Tracker and Glove Demo by Noah Zerkin (noazark AT gmail.com) 2009
Released under GNU GPLv3.0 http://www.opensource.org/licenses/gpl-3.0.html
*/

void gloveTilt(int s){
  // float gXMap = map(gloveX[s], gloveXChipLow[s], gloveXChipHigh[s], -90, 90);
  /*
  float gXMap = map(gloveX[s], 38, 127, -90, 90);
  float gYMap = map(gloveY[s], gloveYChipLow[s], gloveYChipHigh[s], -90, 90);
  float gZMap = map(gloveZ[s], gloveZChipLow[s], gloveZChipHigh[s], -90, 90);
  gloveRoll[s] = atan2(gXMap,sqrt(gYMap*gYMap + gZMap*gZMap));
  glovePitch[s] = atan2(gYMap,sqrt(gXMap*gXMap + gZMap*gZMap));
  gloveZTilt[s] = atan2(sqrt(gXMap*gXMap + gYMap*gYMap),gZMap);
  */
  /*
  float gXMap = correctGloveVal(gloveX[s]);
  float gYMap = correctGloveVal(gloveY[s]);
  float gZMap = correctGloveZVal(gloveZ[s]);
  */
  
  gXMap[s] = xb + (xm*gloveXVal[s]);
  gYMap[s] = yb + (ym*gloveYVal[s]);
  gZMap[s] = zb + (zm*gloveZVal[s]);
  gloveRoll[s] = atan2(gXMap[s],sqrt(gYMap[s]*gYMap[s] + gZMap[s]*gZMap[s]));
  glovePitch[s] = atan2(gYMap[s],sqrt(gXMap[s]*gXMap[s] + gZMap[s]*gZMap[s]));
  gloveZTilt[s] = atan2(gZMap[s],sqrt(gXMap[s]*gXMap[s] + gYMap[s]*gYMap[s]));
 // gloveQuadrantMap(s);
//   print(gXMap); print("-"); print(gYMap); print("-"); print(gZMap); print("-"); print(gloveRoll[s]); print("-"); print(glovePitch[s]); print("-"); println(gloveZTilt[s]);
}

void gloveQuadrantMap(int s){
  // xTilt
  if (gloveRoll[s]<0){
    if (glovePitch[s]<0){
      realGloveRoll = TWO_PI + gloveRoll[s];
    }
    else{
      realGloveRoll = PI - gloveRoll[s];
    }
  }
  if (gloveRoll[s]>0){
    if (glovePitch[s]<0){
      realGloveRoll = gloveRoll[s];
    }
    else{
      realGloveRoll = PI - gloveRoll[s];
    }
  }
  // yTilt
  if (gloveZTilt[s]<0){
    if (sqrt(sq(gloveZTilt[s])+sq(glovePitch[s]))<0){
      realGlovePitch = TWO_PI + gloveZTilt[s];
    }
    else{
      realGlovePitch = PI - gloveZTilt[s];
    }
  }
  if (glovePitch[s]>0){
    if (sqrt(sq(gloveZTilt[s])+sq(glovePitch[s]))<0){
      realGlovePitch = gloveZTilt[s];
    }
    else{
      realGlovePitch = PI - gloveZTilt[s];
    }
  }
}
